#include <joyos.h>
#include <happylib.h>
#include <math.h>

#include "define.h"
#include "monitor.h"
#include "distance.h"
#include "roboturn.h"

struct lock mon_lock;
int pos[3] = {0,0,0};

void gameMonitor(){
	int (*monitorpt)(void) = monitor;
	init_lock (&mon_lock, "Mon lock");
	create_thread(monitorpt, 1500, 10, "Monitor");
}

int monitor(void){
	while(1){
		if(status == 0){ // or scoring
			yield();
		}else if(status == 1){
			updatePos();
		}else if(status == 2){
			updateDegree();
		}
		pause(50);
	}
	return 0;
}

int shaftToDistance(int16_t shaft){ // inch
	return DIST_PER_WHEEL/SHAFT_PER_WHEEL*shaft;
}

void updatePos(){ //Rely on shaft encoder
	uint16_t nowL = encoder_read(ENCODER_PORT_LEFT);
	uint16_t nowR = encoder_read(ENCODER_PORT_RIGHT);
	int delta_dist = (shaftToDistance(nowL-shaftValueMonitorL) +shaftToDistance(nowR-shaftValueMonitorR))/2;
	
	acquire(&mon_lock);
	shaftValueMonitorL = nowL;
	shaftValueMonitorL = nowR;
	release(&mon_lock);
	pos[0] = pos[0]+(delta_dist*cos(pos[3]));
	pos[1] = pos[1]+(delta_dist*sin(pos[3]));
}

void updateDegree(){ //Rely on gyro
	pos[3] = gyro_get_degrees();
}

int *getRobotPos(){ //[0] = x [1] =y [2] = degree
	return pos;
}
/*
 * @param place
 * 0 = LEFT HOME
 * 1 = LEFT 4
 * 2 = LEFT 2
 * 3 = LEFT 1
 * 4 = SKUNK
 * 5 = RIGHT 1
 * 6 = RIGHT 2 
 * 7 = RIGHT 4
 * 8 = RIGHT HOME
 */
void moveTo(int place){
	
}
void resolveUnfulfillState(){
	//myStrategy();
}

bool foundEnemy(void){
	/*double threshold = 5.5;
	double enemyDistance = getDistance();
	if (enemyDistance <= threshold)
		return true;
	else
		return false;*/
	return false;
}

bool stuck(void){
	uint16_t threshold = 10;
	uint16_t init_left_encoder_val = encoder_read(ENCODER_PORT_LEFT);
	uint16_t init_right_encoder_val = encoder_read(ENCODER_PORT_RIGHT);
	pause(3000);
	uint16_t new_left_encoder_val = encoder_read(ENCODER_PORT_LEFT);
	uint16_t new_right_encoder_val = encoder_read(ENCODER_PORT_RIGHT);
	
	if ((new_left_encoder_val - init_left_encoder_val <= threshold) ||
		(init_left_encoder_val - new_left_encoder_val <= threshold) ||
		(new_right_encoder_val - init_right_encoder_val <= threshold) ||
		(init_right_encoder_val - new_right_encoder_val <= threshold)){
		return true;
	}
	else
		return false;
} 





